# 四足机器人逆运动学计算模块
import math

def ik_single(hu,hl,h,x,y,z):
    """
    hu: 大腿长度
    hl: 小腿长度
    h: 肩部高度
    x: x轴坐标
    y: y轴坐标
    z: z轴坐标
    """
    # 求肩部转角gama
    d=math.sqrt(y*y+z*z)
    l=math.sqrt(d*d-h*h)
    gama2=-math.atan(y/z)
    gama1=-math.atan(-h/l)
    gama=180/math.pi*(gama2-gama1)  #肩部转角

    # 求小腿转角beta
    s=math.sqrt(l*l+x*x)
    n=(s*s-hl*hl-hu*hu)/(2*hu)
    beta=180/math.pi*(-math.acos(n/hl))

    # 求大腿转角alpha
    alpha1=-math.atan(x/l)
    alpha2=math.acos((hu+n)/s)
    alpha=180/math.pi*(alpha2+alpha1)
    
    return gama,alpha,beta    #返回值顺序:肩部，大腿，小腿


def ik(case,legL,x1_c,x2_c,x3_c,x4_c,z1_c,z2_c,z3_c,z4_c,y1_c,y2_c,y3_c,y4_c):
  [leg1_shou,leg1_ham,leg1_shank]=ik_single(legL[0],legL[1],legL[2],x1_c,y1_c,z1_c)
  [leg2_shou,leg2_ham,leg2_shank]=ik_single(legL[0],legL[1],legL[2],x2_c,y2_c,z2_c)
  [leg3_shou,leg3_ham,leg3_shank]=ik_single(legL[0],legL[1],legL[2],x3_c,y3_c,z3_c)
  [leg4_shou,leg4_ham,leg4_shank]=ik_single(legL[0],legL[1],legL[2],x4_c,y4_c,z4_c)

  return leg1_shou,leg1_ham,leg1_shank,leg2_shou,leg2_ham,leg2_shank,leg3_shou,leg3_ham,leg3_shank,leg4_shou,leg4_ham,leg4_shank
